#ifndef _ROUTINES_H
#define _ROUTINES_H

#include <iostream>
#include <memory.h>
#include <fstream>
#include <Eigen/Dense>

#include "PLUSolver.h"
#include "PLUSolver.cpp"

using namespace std;

double eps = 1e5;


int test_GetReverseA(int n){ 

    double scalar = 10;

    Eigen::MatrixXd eA = scalar * Eigen::MatrixXd::Random(n,n);
    eA = eA.transpose() * eA;
    // std::cout << "eA = ------------" <<std::endl << eA << std::endl << std::endl;
    Eigen::VectorXd eB = scalar * Eigen::VectorXd::Random(n);
    eB = eA.transpose() * eB;
    // std::cout << "eB = ------------" <<std::endl << eB << std::endl << std::endl;
    Eigen::VectorXd eX = Eigen::VectorXd(n);
    eX = eA.lu().solve(eB);
    // std::cout << "eX = ------------" <<std::endl << eX << std::endl << std::endl;

    double **A = (double**)malloc(sizeof(double*)*n); // 这里采用二级指针的方式进行存储，为了方便，行与行之间的交换
    double *B = (double*)malloc(sizeof(double) * n);
    for(int i = 0; i < n; i++){
        A[i] =(double*)malloc(sizeof(double)*n);
    }

    for (int i = 0; i < n; i++){
        for (int j = 0; j < n; j++){
            A[i][j] = eA(i, j);
        }
        B[i] = eB(i);
    }

    int *P = (int*)malloc(sizeof(int)*n);
    double *X = (double*)malloc(sizeof(double)*n);
    PLUSolver<double> test(A, n, B, P);
    test.PLU();
    test.Solve(X);

    double** rA = (double**)malloc(sizeof(double*) * n);
    for(int i = 0; i < n; i++){
        rA[i] = (double*)malloc(sizeof(double) * n);
    }

    test.GetReverseA(rA);

    // check
    Eigen::MatrixXd erA = eA.inverse(); 
    int error = 0;
    for (int i = 0; i < n; i++){
        for(int j = 0; j < n; j++){
            if(abs(rA[i][j] - erA(i, j)) > eps){
                error = 1;
            }
        }
    }

    // delete
    for(int i = 0; i < n; i++){
        free(A[i]);
    }
    free(A);
    free(B);
    free(P);
    free(X);

    if(error) return 0;
    else return 1;


}


int test_solve(int n){ 

    double scalar = 10;

    Eigen::MatrixXd eA = scalar * Eigen::MatrixXd::Random(n,n);
    eA = eA.transpose() * eA;
    // std::cout << "eA = ------------" <<std::endl << eA << std::endl << std::endl;
    Eigen::VectorXd eB = scalar * Eigen::VectorXd::Random(n);
    eB = eA.transpose() * eB;
    // std::cout << "eB = ------------" <<std::endl << eB << std::endl << std::endl;
    Eigen::VectorXd eX = Eigen::VectorXd(n);
    eX = eA.lu().solve(eB);
    // std::cout << "eX = ------------" <<std::endl << eX << std::endl << std::endl;

    double **A = (double**)malloc(sizeof(double*)*n); // 这里采用二级指针的方式进行存储，为了方便，行与行之间的交换
    double *B = (double*)malloc(sizeof(double) * n);
    for(int i = 0; i < n; i++){
        A[i] =(double*)malloc(sizeof(double)*n);
    }

    for (int i = 0; i < n; i++){
        for (int j = 0; j < n; j++){
            A[i][j] = eA(i, j);
        }
        B[i] = eB(i);
    }

    int *P = (int*)malloc(sizeof(int)*n);
    double *X = (double*)malloc(sizeof(double)*n);
    PLUSolver<double> test(A, n, B, P);
    test.PLU();
    test.Solve(X);

    std::cout<<"!!"<<std::endl;

    // check
    int error = 0;
    for(int i = 0; i < n; i++){
        if(abs(X[i] - eX(i)) > eps){
            error = 1;
        }
    }

    // delete
    for(int i = 0; i < n; i++){
        free(A[i]);
    }

    free(A);
    free(B);
    free(P);
    free(X);

    if(error) return 0;
    else return 1;
}

void matrixFromIntxt(std::string filepath){
    fstream in;
    in.open(filepath, ios::in);
    int n;
    in>>n;
    double **A = (double**)malloc(sizeof(double*)*n);
    for(int i = 0; i < n; i++){
        A[i] =(double*)malloc(sizeof(double)*n);
    }
    double *B = (double*)malloc(sizeof(double) * n);
    int *P = (int*)malloc(sizeof(int)*n);
    double *X = (double*)malloc(sizeof(double)*n);
    double temp;
    for(int i = 0; i < n; i++){
        for(int j = 0; j < n; j++){
            in>>temp;
            A[i][j] = (double)1.0*temp;
        }
    }
    for(int i = 0; i < n; i++){
        in>>B[i];
    }

    PLUSolver<double> solver(A, n, B, P);

    solver.PrintMatrix(solver.A, n, "oringin A");

    solver.PLU();
    solver.Solve(X);

    solver.PrintMatrix(solver.A, n, "after LU decomposition compressed A");

    solver.PrintVector(X, n, "print solution x");

    double** rA = (double**)malloc(sizeof(double*) * n);
    for(int i = 0; i < n; i++){
        rA[i] = (double*)malloc(sizeof(double) * n);
    }
    solver.GetReverseA(rA);

    solver.PrintMatrix(solver.rA, n, "rA");

    free(A);
    free(B);
    free(P);
    free(X);
}


void test_swap(int n){

    double **A = (double**)malloc(sizeof(double*)*n); // 这里采用二级指针的方式进行存储，为了方便，行与行之间的交换
    double *B = (double*)malloc(sizeof(double) * n);
    for(int i = 0; i < n; i++){
        A[i] =(double*)malloc(sizeof(double)*n);
        for(int j = 0; j < n; j++){
            A[i][j] = (double)(1.0)*i;    
        }
        B[i] = (double)1.0*n;
    }
    int *P = (int*)malloc(sizeof(int)*n);
    double *X = (double*)malloc(sizeof(double)*n);
    PLUSolver<double> test(A, n, B, P);

    test.swap(1,2);
    for(int i = 0; i < n; i++){
        for(int j =0; j < n; j++){
            printf("%lf ", A[i][j]);
        }
        printf("%d \n", P[i]);
    }

}


#endif